package com.zkyt.lib_msdk_ext.component

import com.geoai.mavlink.geoainet.flycontroller.info.FlyControllerStateInfo
import com.geoai.mavlink.geoainet.gimbal.info.GimbalStateInfo
import com.geoai.mavlink.geoainet.gimbal.interfaces.IBaseGimbalManager
import com.zkyt.lib_msdk_ext.common.SDKExtUtil
import io.reactivex.Completable
import io.reactivex.subjects.BehaviorSubject
import java.util.Optional

/**
 * Created by chenyu on 2024/2/23
 * Description:
 */
object GimbalManagerExt: AbsSDKExt<IBaseGimbalManager>() {
    private var orgManager: IBaseGimbalManager? = null

    private var orgGimbalStateObservable =
        BehaviorSubject.createDefault<Optional<GimbalStateInfo>>(Optional.ofNullable(null))

    override fun init(originManager: IBaseGimbalManager) {
        orgManager = originManager

        orgManager?.setGimbalStateListener {
            orgGimbalStateObservable.onNext(Optional.of(it))
        }
    }

    override fun destroy() {
        super.destroy()
        orgManager?.setGimbalStateListener(null)
        orgGimbalStateObservable.onNext(Optional.ofNullable(null))
    }

    override fun getOriginManager(): IBaseGimbalManager? {
        return orgManager
    }

    public fun getGimbalStateObservable(): BehaviorSubject<Optional<GimbalStateInfo>> {
        return orgGimbalStateObservable
    }

    fun setGimbalPitchAngleRx(angle: Float): Completable {
        return Completable.create { emitter ->
            orgManager?.setGimbalPitch(angle) {
                SDKExtUtil.handleError(emitter, it)
            }
        }
    }

    fun setGimbalYawAngleRx(angle: Float): Completable {
        val pitchAngle = getGimbalStateObservable().value?.get()?.yawAngle?:0.0f
        return Completable.create { emitter ->
            orgManager?.setGimbalAngleControl(false, pitchAngle, angle) {
                SDKExtUtil.handleError(emitter, it)
            }
        }
    }

    fun resetGimbalRx(): Completable {
        return Completable.create { emitter ->
            orgManager?.resetGimbal {
                SDKExtUtil.handleError(emitter, it)
            }
        }
    }

    fun resetGimbalAndDownRx(): Completable {
        return Completable.create { emitter ->
            orgManager?.resetGimbalAndDown {
                SDKExtUtil.handleError(emitter, it)
            }
        }
    }
}